
#ifndef SINGLESWEEPANDPRUNE_H_
#define SINGLESWEEPANDPRUNE_H_

#include "BroadPhase.h"

#define SAP_OBJECT_RESERVE		10000

namespace p2 {

struct SapEndPoint
{
	SapEndPoint()
	: _val(-Math<real>::MAX_REAL)
	{
	}
	SapEndPoint(real value)
    : _val(value)
	{
	}
    SapEndPoint(SapEndPoint const& val)
    : _data(val._data),
      _val(val._val)
    {
    }
	SapEndPoint& operator=(SapEndPoint const& val)
	{
		if (&val != this) {
			_data = val._data;
			_val = val._val;
		}
		return *this;
	}
	void setIsMin(bool isMin)
	{
		_data.ismin = isMin;
	}
	bool isMin()
	{
		return _data.ismin;
	}
	void setBoxId(uint32 boxId)
	{
		_data.boxId = boxId;
	}
	uint32 boxId()
	{
		return _data.boxId;
	}
	void setMarkedForDeletion()
	{
		_data.boxId = 0x7fffffff;
	}
	bool isMarkedForDeletion()
	{
		return (_data.boxId == 0x7fffffff);
	}
	/*
	 * data
	 */
	struct Data
	{
		unsigned boxId :31;
		unsigned ismin :1;
	};
    
	Data _data;
	real _val;
};

struct SapObject
{
	inline SapObject()
    : _plane(0), _data(NULL)
	{
		_iMin[0] = _iMin[1] = _iMax[0] = _iMax[1] = NULL_INDEX;
	}
	int32 _iMin[2];
	int32 _iMax[2];
	uint32 _plane;
	void* _data;
};

struct SapPair
{
	inline SapPair();
	inline SapPair(uint16 id0, uint16 id1);
	int16 _id0, _id1;
};

typedef boost::unordered_map<uint32, SapPair> SapPairMap;
typedef std::pair<uint32, SapPair> SapPairMapElement;
class SapPairManager
{
public:
	SapPairManager()
	{
		_active.rehash(SAP_OBJECT_RESERVE);
	}
	SapPair* add(uint16 id0, uint16 id1)
	{
		uint32 key = packedIndex(id0, id1);
		if (_active.find(key) == _active.end()) {
			_active[key] = SapPair(id0, id1);
		}
		return &_active[key];
	}
	void remove(uint16 id0, uint16 id1)
	{
		uint32 key = packedIndex(id0, id1);
		SapPairMap::iterator iter = _active.find(key);
		if (iter != _active.end()) {
			_active.erase(iter);
		}
	}
	inline SapPair* find(uint16 id0, uint16 id1)
	{
		SapPairMap::iterator iter = _active.find(packedIndex(id0, id1));
		if (iter != _active.end()) {
			return &iter->second;
		}
		return NULL;
	}
	static inline uint32 packedIndex(uint16 id0, uint16 id1)
	{
		return
        id0 > id1 ? (uint32) (id0 << 16) | id1 : (uint32) (id1 << 16) | id0;
	}
	inline uint32 packedIndex(SapPair* pair)
	{
		return SapPairManager::packedIndex(pair->_id0, pair->_id1);
	}
	inline SapPairMap& active()
	{
		return _active;
	}
    
private:
	SapPairMap _active;
};

class SingleSweepAndPrune: public BroadPhase
{
public:
	inline SingleSweepAndPrune();
	virtual ~SingleSweepAndPrune();
    
	virtual void updateNotify();
    
	virtual uint32 addObject(Aabb2<real> const& box, uint32 plane, void* userData, bool commitObject = true);
	virtual void removeObject(uint32 objId);
	virtual void updateObject(Aabb2<real> const& box, uint32 objId);
	inline SapPairManager& pairManager();
	inline SapObject& getObject(uint32 id);
    
#ifdef DEBUG
	void logMetrics();
	void drawEndPoints();
#endif
	static bool endPointBatchSortPredicate(SapEndPoint const& ep0,
                                           SapEndPoint const& ep1);
    
private:
	void commitBatchInsert();
	void commitBatchDelete();
	inline bool overlap(uint32 objId0, uint32 objId1);
	inline void swapEndPoints(uint32 iA, uint32 iB, uint32 axis);
	void checkEndPointBubbleUp(uint32 endPointIndex, uint32 axis);
	void checkEndPointBubbleDown(uint32 endPointIndex, uint32 axis);
    
#ifdef DEBUG
	void dumpEndPoints();
#endif
    
private:
	uint32 _batchStartObjectId;
	std::vector<SapObject> _objects;
	std::vector<SapEndPoint> _endPoints[2]; // min->max ordering for both axes
	std::vector<SapEndPoint> _axisBatch[2]; // batch for each axis
	SapPairManager _pairManager;
};

SapPair::SapPair()
: _id0(NULL_INDEX), _id1(NULL_INDEX)
{
}

SapPair::SapPair(uint16 id0, uint16 id1)
: _id0(id0), _id1(id1)
{
}

SingleSweepAndPrune::SingleSweepAndPrune()
: _batchStartObjectId(0)
{
	SapEndPoint minLimit(-Math<real>::MAX_REAL);
	SapEndPoint maxLimit(+Math<real>::MAX_REAL);
	for (uint32 axis = 0; axis < 2; axis++) {
		_endPoints[axis].reserve(SAP_OBJECT_RESERVE);
		_endPoints[axis].push_back(minLimit);
		_endPoints[axis].push_back(maxLimit);
	}
}

inline bool SingleSweepAndPrune::overlap(uint32 obj0, uint32 obj1)
{
	if ((_objects[obj0]._plane&_objects[obj1]._plane) == 0) {
		/*
		 * objects not on the same collision plane
		 */
		return false;
	}
	if ((_endPoints[0][_objects[obj1]._iMin[0]]._val
         - _endPoints[0][_objects[obj0]._iMax[0]]._val) > 0.f) {
		return false;
	}
	if ((_endPoints[1][_objects[obj1]._iMin[1]]._val
         - _endPoints[1][_objects[obj0]._iMax[1]]._val) > 0.f) {
		return false;
	}
	if ((_endPoints[0][_objects[obj0]._iMin[0]]._val
         - _endPoints[0][_objects[obj1]._iMax[0]]._val) > 0.f) {
		return false;
	}
	if ((_endPoints[1][_objects[obj0]._iMin[1]]._val
         - _endPoints[1][_objects[obj1]._iMax[1]]._val) > 0.f) {
		return false;
	}
	return true;
}

inline void SingleSweepAndPrune::swapEndPoints(uint32 iA, uint32 iB,
                                               uint32 axis)
{
	if (_endPoints[axis][iA]._data.ismin) {
		_objects[_endPoints[axis][iA].boxId()]._iMin[axis] = iB;
	} else {
		_objects[_endPoints[axis][iA].boxId()]._iMax[axis] = iB;
	}
	if (_endPoints[axis][iB]._data.ismin) {
		_objects[_endPoints[axis][iB].boxId()]._iMin[axis] = iA;
	} else {
		_objects[_endPoints[axis][iB].boxId()]._iMax[axis] = iA;
	}
	std::swap(_endPoints[axis][iA], _endPoints[axis][iB]);
}

inline SapPairManager& SingleSweepAndPrune::pairManager()
{
	return _pairManager;
}

inline SapObject& SingleSweepAndPrune::getObject(uint32 id)
{
	return _objects[id];
}

}

#endif	//SINGLESWEEPANDPRUNE_H_
